#include "AStar/AStarVector3.h"
#include <cfloat>

namespace PocketSurvival
{
    void AStarVector3::buildPathFromEndNode(std::shared_ptr<NodeVector3> startNode, std::shared_ptr<NodeVector3> endNode)
    {
        PathCost = endNode->G;
        Path->clear();
        for (std::shared_ptr<NodeVector3> node = endNode; node != startNode; node = ClosedStorage.get(node->PreviousPosition))
        {
            Path->push_back(node->Position);
        }
    }

    void AStarVector3::findPath(const Vector3 &start, const Vector3 &end, float minHeuristic, int32_t maxPositionsToCheck)
    {
        m_nodesCacheIndex = 0;
        m_openHeap.clear();
        OpenStorage.clear();
        ClosedStorage.clear();
        std::shared_ptr<NodeVector3> node = newNode(start, FastVector3::Zero, 0.0f, 0.0f);
        OpenStorage.set(start, node);
        heapEnqueue(node);
        std::shared_ptr<NodeVector3> node2 = nullptr;
        int num = 0;
        std::shared_ptr<NodeVector3> node3;
        while (true)
        {
            node3 = (m_openHeap.size() > 0) ? heapDequeue() : nullptr;
            if (node3 == nullptr || num >= maxPositionsToCheck)
            {
                if (node2 != nullptr)
                {
                    buildPathFromEndNode(node, node2);
                    return;
                }
                Path->clear();
                PathCost = 0.0f;
                return;
            }
            if (World.IsGoal(node3->Position))
            {
                break;
            }
            ClosedStorage.set(node3->Position, node3);
            OpenStorage.set(node3->Position, nullptr);
            num++;
            m_neighbors.clear();
            World.getNeighbors(node3->Position, m_neighbors);
            for (const Vector3 &val : m_neighbors)
            {
                if (ClosedStorage.get(val) != nullptr)
                {
                    continue;
                }
                float num2 = World.cost(node3->Position, val);
                if (num2 == FLT_MAX)
                {
                    continue;
                }
                float num3 = node3->G + num2;
                float num4 = World.heuristic(val, end);
                if (node3 != node && (node2 == nullptr || num4 < node2->H))
                {
                    node2 = node3;
                }
                auto node4 = OpenStorage.get(val);
                if (node4 != nullptr)
                {
                    if (num3 < node4->G)
                    {
                        node4->G = num3;
                        node4->F = num3 + node4->H;
                        node4->PreviousPosition = node3->Position;
                        heapUpdate(node4);
                    }
                }
                else
                {
                    node4 = newNode(val, node3->Position, num3, num4);
                    OpenStorage.set(val, node4);
                    heapEnqueue(node4);
                }
            }
        }
        buildPathFromEndNode(node, node3);
    }

    void AStarVector3::heapEnqueue(std::shared_ptr<NodeVector3> node)
    {
        m_openHeap.push_back(node);
        heapifyFromPosToStart(m_openHeap.size() - 1);
    }

    std::shared_ptr<NodeVector3> AStarVector3::heapDequeue()
    {
        std::shared_ptr<NodeVector3> result = m_openHeap[0];
        if (m_openHeap.size() <= 1)
        {
            m_openHeap.clear();
            return result;
        }
        m_openHeap[0] = *(m_openHeap.rbegin());
        m_openHeap.erase(m_openHeap.rbegin().base());
        heapifyFromPosToEnd(0);
        return result;
    }

    void AStarVector3::heapUpdate(std::shared_ptr<NodeVector3> node)
    {
        int32_t pos = -1;
        for (int32_t i = 0; i < m_openHeap.size(); i++)
        {
            if (m_openHeap[i] == node)
            {
                pos = i;
                break;
            }
        }
        heapifyFromPosToStart(pos);
    }

    void AStarVector3::heapifyFromPosToEnd(int32_t pos)
    {
        while (true)
        {
            int32_t num = pos;
            int32_t num2 = 2 * pos + 1;
            int32_t num3 = 2 * pos + 2;
            if (num2 < m_openHeap.size() && m_openHeap[num2]->F < m_openHeap[num]->F)
            {
                num = num2;
            }
            if (num3 < m_openHeap.size() && m_openHeap[num3]->F < m_openHeap[num]->F)
            {
                num = num3;
            }
            if (num != pos)
            {
                std::shared_ptr<NodeVector3> node = m_openHeap[num];
                m_openHeap[num] = m_openHeap[pos];
                m_openHeap[pos] = node;
                pos = num;
                continue;
            }
            break;
        }
    }

    void AStarVector3::heapifyFromPosToStart(int32_t pos)
    {
        int32_t num = pos;
        while (num > 0)
        {
            int32_t num2 = (num - 1) / 2;
            std::shared_ptr<NodeVector3> node = m_openHeap[num2];
            std::shared_ptr<NodeVector3> node2 = m_openHeap[num];
            if (node->F > node2->F)
            {
                m_openHeap[num2] = node2;
                m_openHeap[num] = node;
                num = num2;
                continue;
            }
            break;
        }
    }

    std::shared_ptr<NodeVector3> AStarVector3::newNode(const Vector3 &position, const Vector3 &previousPosition, float g, float h)
    {
        while (m_nodesCacheIndex >= m_nodesCache.size())
        {
            m_nodesCache.push_back(std::make_shared<NodeVector3>());
        }
        std::shared_ptr<NodeVector3> obj = m_nodesCache[m_nodesCacheIndex++];
        obj->Position = position;
        obj->PreviousPosition = previousPosition;
        obj->F = g + h;
        obj->G = g;
        obj->H = h;
        return obj;
    }

} // namespace PocketSurvival
